//
//  verificationEarlyTermination.cpp
//  map-matching
//
//  Created by Lv Xin on 16/5/10.
//  Copyright © 2016年 Lv Xin. All rights reserved.
//
//  Personal Homepage: www.davidlvxin.com

#include "verificationEarlyTermination.h"
#include "dijkstra.h"
#include "trajectory_generated.h"
#include "distance.h"
#include <queue>

using namespace std;

extern std::vector< std::vector<EdgeDis> > candidateEdge;
extern std::list<Route*> candidateR;
extern std::list<Route*> candidateRAfterTreeCut;

vector<int> findpath(int node1, int node2);

EarlyTermination::EarlyTermination(bool u, Map* m, Trajectary& tra) : Verification(u), traj(tra){
    int T = candidateEdge.size();
    mapSet = m;
    shortestPath = new std::vector<int>** [T - 1];
    for(int i = 0; i < T - 1; i++){
        int sizeA = candidateEdge[i].size();
        shortestPath[i] = new std::vector<int>* [sizeA];
        for(int j = 0; j < sizeA; j++){
            int sizeB = candidateEdge[i + 1].size();
            shortestPath[i][j] = new std::vector<int> [sizeB];
        }
    }
    if(!useStrategy)
        sortCandidateRByPQ();
    maxSimPoint = maxSimPointB = -INF;
    maxSimLenR = INF;
}

EarlyTermination::~EarlyTermination(){
    int size = candidateEdge.size();
    for(int i = 0; i < size - 1; i++){
        int sizeA = candidateEdge[i].size();
        for(int j = 0; j < sizeA; j++){
            delete [] shortestPath[i][j];
        }
        delete [] shortestPath[i];
    }
    delete [] shortestPath;
    deleteRoute();
}

void EarlyTermination::sortCandidateRByPQ(){
    priority_queue<RouteWithSima*, std::vector<RouteWithSima*>, cmp> PQ;
    list<Route*>::iterator s;
    for(s = candidateR.begin(); s != candidateR.end(); s++){
        RouteWithSima* tmp = new RouteWithSima(*s);
        PQ.push(tmp);
    }
    candidateR.clear();
    while(!PQ.empty()){
        candidateR.push_back(PQ.top()->m);
        delete PQ.top();
        PQ.pop();
    }
}

void EarlyTermination::computeSim(Route * route){
    double allLenRoute = 0;
    int size = candidateEdge.size();
    GenerateTrajectory temp;
    int firstSize = candidateEdge[0][route->rank[0]].edge.size();
    DoublePos post;
    PosWithRank r;
    post.lat = candidateEdge[0][route->rank[0]].edge[0].beginLat;
    post.lng = candidateEdge[0][route->rank[0]].edge[0].beginLng;
    r.pos = post;
    r.rank = candidateEdge[0][route->rank[0]].edge[0].begin;
    temp.point.push_back(r);
    for(int i = 0; i < firstSize; i++){
        DoublePos pos;PosWithRank r;
        pos.lat = candidateEdge[0][route->rank[0]].edge[i].endLat;
        pos.lng = candidateEdge[0][route->rank[0]].edge[i].endLng;
        r.pos = pos; r.rank = candidateEdge[0][route->rank[0]].edge[i].end;
        allLenRoute += distance2meter(temp.point[temp.point.size() - 1].pos.lat, temp.point[temp.point.size() - 1].pos.lng, r.pos.lat, r.pos.lng);
        if(allLenRoute > maxSimLenR) return;
        temp.point.push_back(r);
    }
    for(int i = 1; i < size; i++){
        if(temp.point[temp.point.size() - 1].pos.lat != candidateEdge[i][route->rank[i]].edge[0].beginLat || temp.point[temp.point.size() - 1].pos.lng != candidateEdge[i][route->rank[i]].edge[0].beginLng){//如果需要最短路
            vector<int> path;
            path.clear();
            if(shortestPath[i][route->rank[i - 1]][route -> rank[i]].size() == 0){
                path = findpath(temp.point[temp.point.size() - 1].rank, candidateEdge[i][route->rank[i]].edge[0].begin);
                shortestPath[i][route->rank[i - 1]][route -> rank[i]] = path;
            }else{
                path = shortestPath[i][route->rank[i - 1]][route -> rank[i]];
            }
            int pathSize = path.size();
            for(int j = 0; j < pathSize; j++){
                DoublePos pp; PosWithRank rin;
                pp.lat = mapSet->node[path[j]].lat;
                pp.lng = mapSet->node[path[j]].lng;
                rin.pos = pp; rin.rank = mapSet->node[path[j]].rank;
                allLenRoute += distance2meter(temp.point[temp.point.size() - 1].pos.lat, temp.point[temp.point.size() - 1].pos.lng, rin.pos.lat, rin.pos.lng);
                if(allLenRoute > maxSimLenR) return;
                temp.point.push_back(rin);
            }
        }
        int RankSize = candidateEdge[i][route->rank[i]].edge.size();
        DoublePos postt;
        PosWithRank rr;
        postt.lat = candidateEdge[i][route->rank[i]].edge[0].beginLat;
        postt.lng = candidateEdge[i][route->rank[i]].edge[0].beginLng;
        rr.pos = post;
        rr.rank = candidateEdge[i][route->rank[i]].edge[0].begin;
        temp.point.push_back(rr);
        for(int j = 0; j < RankSize; j++){
            DoublePos poss;PosWithRank rs;
            poss.lat = candidateEdge[i][route->rank[i]].edge[j].endLat;
            poss.lng = candidateEdge[i][route->rank[i]].edge[j].endLng;
            rs.pos = poss; rs.rank = candidateEdge[i][route->rank[i]].edge[j].end;
            allLenRoute += distance2meter(temp.point[temp.point.size() - 1].pos.lat, temp.point[temp.point.size() - 1].pos.lng, rs.pos.lat, rs.pos.lng);
            if(allLenRoute > maxSimLenR) return;
            temp.point.push_back(rs);
        }
    }
    TR_SIM sim(temp, traj);
    double dijSum = 0.0;
    for(int i = 0; i < size; i++){
        dijSum += candidateEdge[i][route->rank[i]].dis;
    }
    sim.setPart1(1 - dijSum / size);
    sim.computeSim();
    if(sim.getSim() > maxSimPoint){
        maxSimPoint = sim.getSim();
        maxSimPointB = sim.getSimPart2();
        maxSimRoute = temp;
        maxSimLenR = sim.getLenR();
    }
}

void EarlyTermination::computeWithStrategy(){
    list<Route*>::iterator s;
    for(s = candidateRAfterTreeCut.begin(); s != candidateRAfterTreeCut.end(); s++){
        computeSim(*s);
    }
}

void EarlyTermination::computeWithoutStrategy(){
    list<Route*>::iterator s;
    for(s = candidateR.begin(); s != candidateR.end(); s++){
        computeSim(*s);
    }
}

void EarlyTermination::computeMaxSim(){
    if(useStrategy)
        computeWithStrategy();
    else
        computeWithoutStrategy();
}

void EarlyTermination::deleteRoute(){
    list<Route*>::iterator s;
    for(s = candidateR.begin(); s != candidateR.end(); s++){
        delete (*s);
    }
    candidateR.clear();
    candidateRAfterTreeCut.clear();
}

GenerateTrajectory EarlyTermination::getMaxSim(){
    return maxSimRoute;
}